#include "foc.h"
// #include "tim.h"
#include "abz_encoder.h"
#include "math.h"
#include "cordic.h"
#include "Lowpass_Filter.h"
#include "pid.h"
#include "hall_sensor.h"
#include "motor.h"

#define PI         3.14159265359f
#define _3PI_2     4.71238898f  // 辨识零点电角度时使用

struct LOWPASS M_Cur_Filter = {0.1};

PID pid = {0};

float Uu=0,Uv=0,Uw=0,Ualpha,Ubeta=0,dc_a=0,dc_b=0,dc_c=0;
float Ua=0,Ub=0,Uc=0;
float voltage_limit = 23;             // 限压
float voltage_power_supply = 24   ;   // 母线电压
float zero_electric_Angle= 2.540272f;	// 大电机 零点电角度
// float zero_electric_Angle = -0.445877;		// 小电机1 零点电角度
// float zero_electric_Angle = 2.493074;		// 小电机2 HALL零点电角度
//  float zero_electric_Angle = -1.445877;		// 小电机2 零点电角度
// float zero_electric_Angle = 1.513372; // 小电机2 ABZ零点电角度
// float zero_electric_Angle = 1.8f;
float sensor_angle = 0;   // 传感器角度
int Dir = 1;  // 减速比
int pp = 1;   // 极对数
float I_q = 0.0f;
float I_d = 0.0f;
float Vd = 0.0f;

// 有刷直流电机
void SetDCVoltage(float U)
{
	float Vmd = voltage_power_supply / 2;						// 中心电压为电源电压的二分之一
	SetPwm(Vmd + U / 2, Vmd - U / 2, 0);								// 取 UVW当中的UV两相作为直流电机的正负极
}

// 无刷电机
void SetPhaseVoltageSVPWM(float Uq, float Ud, float angle_el)
{
	
	// Ualpha = -Uq*sin(angle_el);
	// Ubeta = Uq*cos(angle_el);
	Ualpha = -Uq*Calculate_Sine(angle_el);
	Ubeta = Uq*Calculate_Cosine(angle_el);
	// 计算以0为中心电压的三相正弦波
	Ua = Ualpha;
	Ub = (sqrt(3)*Ubeta - Ualpha) / 2;
	Uc = -(Ualpha + sqrt(3)*Ubeta) / 2;

	// 计算最大值与最小值相比中心轴的偏移量
	float Vmax = fmax(Ua, fmax(Ub, Uc));
	float Vmin = fmin(Ua, fmin(Ub, Uc));
	float Vd = (Vmax + Vmin) / 2;								// 实际中心电压
	float Vd_Prime = Vmax - Vmin;					// 电压幅值

	// printf("Vd: %f, Vd_Prime: %f\n", Vd, Vd_Prime);

	// 用实际的中心电压修正三相正弦波，使实际的中心电压永远为0
	Ua = Ua - Vd;
	Ub = Ub - Vd;
	Uc = Uc - Vd;
	
	// 增幅 移动中心电压
	if (Vd_Prime > 0)
	{
		float Vmd = voltage_power_supply / 2;						// 目标中心电压为电源电压的二分之一
		float zfbl = 1.1547;		// 增幅比例
		Ua = Ua * zfbl + Vmd;
		Ub = Ub * zfbl + Vmd;
		Uc = Uc * zfbl + Vmd;
	} else {
		Ua = 0;
		Ub = 0;
		Uc = 0;
	}
	// printf("Ua: %f, Ub: %f, Uc: %f\n", Ua, Ub, Uc);
	// 限幅
	Uu = fmax(0, fmin(voltage_power_supply, Ua));
	Uv = fmax(0, fmin(voltage_power_supply, Ub));
	Uw = fmax(0, fmin(voltage_power_supply, Uc));

	// 输出
	SetPwm(Uu, Uv, Uw);
}

void SetPwm(float Ua, float Ub, float Uc)
{
	float U_a=0.0;
	float U_b=0.0;
	float U_c=0.0;
	
	U_a = constrain(Ua, 0.0f, voltage_limit);
	U_b = constrain(Ub, 0.0f, voltage_limit);
	U_c = constrain(Uc, 0.0f, voltage_limit);
	
	dc_a = constrain(U_a / voltage_power_supply, 0.0f, 1.0f);
	dc_b = constrain(U_b / voltage_power_supply, 0.0f, 1.0f);
	dc_c = constrain(U_c / voltage_power_supply, 0.0f, 1.0f);
	
	PWM_Channel1(dc_a * 5311);
	PWM_Channel2(dc_b * 5311);
	PWM_Channel3(dc_c * 5311);
	
  // printf("dc_a: %f, dc_b: %f, dc_c: %f\n", dc_a, dc_b, dc_c);
}

// 限制赋值
float constrain(float amt, float low, float high)
{
	 return ((amt<low)?(low):((amt)>(high)?(high):(amt)));
}

// 辨识零点电角度
void Check_Sensor()
{
	printf("init...\n");
  CalSensorAngle();
  SetPhaseVoltageSVPWM((float)2.0f, (float)0.0f, _3PI_2);
  osDelay(2000);
  CalSensorAngle();
	// zero_electric_Angle = 2.540272f;
	zero_electric_Angle = electricAngle();
	// zero_electric_Angle = -0.445877;
  printf("zero_electric_Angle: %f\n", zero_electric_Angle);
  SetPhaseVoltageSVPWM(0, 0, electricAngle());
	
	
	for (uint8_t i = 0; i < 10; i++)
	{
		osDelay(100);
		calCurrentUVW(adc_U, adc_V);
		current_u_offset += current_u;
		current_v_offset += current_v;
		current_w_offset += current_w;
		printf("%d : current_u_offset: %f, current_v_offset: %f, current_w_offset: %f\n", i, current_u_offset, current_v_offset, current_w_offset);
	}
	osDelay(100);
	current_u_offset = current_u_offset / 10;
	current_v_offset = current_v_offset / 10;
	current_w_offset = current_w_offset / 10;
	printf("init finish : current_u_offset: %f, current_v_offset: %f, current_w_offset: %f\n", current_u_offset, current_v_offset, current_w_offset);
	osDelay(500);
}

void Motor_Run()
{
  // 计算角度
  CalSensorAngle();
  float electric_angle = electricAngle();
  calIqId(current_u, current_v, electric_angle);
  // SetPhaseVoltageSVPWM(Vd, (float)0.0f, electric_angle);
	SetPhaseVoltageSVPWM(1.0f, (float)0.0f, electric_angle);
}

// 计算零点电角度
float electricAngle(void)
{
	return (sensor_angle * pp * Dir - zero_electric_Angle);
}

// 计算传感器角度
void CalSensorAngle()
{
	if (encoder_angle_valid)
	{
		// 电机正常
		LED7_OFF();
		uint16_t encode = Read_Encode(); // 0 ~ 4095
		// printf("encode:%d\n", encode);
		sensor_angle = ((float)encode / 4096.0f) * 2.0f * PI;
	} else
	{
		// 电机初始化
		LED7_ON();
		sensor_angle = Hall_Angle();
	}
	// uint16_t encode = Read_Encode(); // 0 ~ 4095
	// printf("encode:%d\n", encode);
	// sensor_angle = ((float)encode / 4096.0f) * 2.0f * PI;
	// sensor_angle = Hall_Angle();
	// printf("sersor_angle: %f\n", sensor_angle);
	// printf("encode: %d\n", encode);
	// printf("hall: %f\n", Hall_Angle());
	
	// sensor_angle = Hall_Angle();
//	printf("encode: %d ", Read_Encode());
//	printf("sensor_angle: %f ", sensor_angle);
//	printf("hall: %f\n", Hall_Angle());
}

// 通过Iu ,Iv, Iw 计算Iq, Id 返回Iq

float calIqId(float current_u, float current_v, float angle_el)
{
  float I_alpha = current_u;
  float I_beta = (1.0f/sqrt(3)) * current_u + (2.0f/sqrt(3)) * current_v;
  float ct = Calculate_Cosine(angle_el);
  float st = Calculate_Sine(angle_el);
  // float ct = cosf(angle_el);
  // float st = sinf(angle_el);
	I_d = I_alpha * ct + I_beta * st;

	// printf("Calculate_Cosine: %f, cosf: %f\n", Calculate_Cosine(angle_el), cosf(angle_el));
  return I_beta * ct - I_alpha * st;

  // printf("U: %f, V: %f, angle: %f, I_q: %f, I_d: %f\n",current_u, current_v, angle_el, I_q, I_d);

	// printf("%f,%f,%f,%f,%f\n",current_u, current_v, current_w, I_q, angle_el);

  // printf("Ua: %f, Ub: %f, Ca: %f, Cb: %f\n", Ua - 6, Ub - 6, current_u, current_v);
}

// 电流环
void Set_CurTorque(float Target)
{
	// 计算传感器角度
	CalSensorAngle();
	// 计算电角度
  float electric_angle = electricAngle();
	// 计算Iq和滤波
	I_q = Lowpassfilter(&M_Cur_Filter, calIqId(current_u - current_u_offset, current_v - current_v_offset, electric_angle));
	// I_q = Lowpassfilter(&M_Cur_Filter, calIqId(current_u, current_v, electric_angle));
	// 位置模式电流环
	// pid.kp = 1.0f;
	// pid.ki = 0.00f;
	// pid.maxIntegral = 0.6;
	// pid.maxOutput = 1.5;

	// 力矩模式电流环
	pid.kp = 2.0f;
	pid.ki = 0.01f;
	pid.maxIntegral = 1.0;
	pid.maxOutput = 3.0;

	PID_Calc(&pid, Target, I_q);
	if (pid.output > 3.0f)
	{
		Vd = 3.0f;
	} else if (pid.output < -3.0f)
	{
		Vd = -3.0f;
	} else
	{
		Vd = pid.output;
	}
	
	// if (motor_target.ctrl_mode == MOTOR_CTRL_OPEN_LOOP)		// 开环控制
	// {
	// 	SetPhaseVoltageSVPWM(motor_target.voltage_d, 0, electricAngle());
	// }
	
	SetPhaseVoltageSVPWM(0.5f, 0, electricAngle());

	// 增加对有刷直流电机的适配

}

// U: 0.728026, V: 0.298119, angle: -4.750739, I_q: 0.698177, I_d: -0.791914
// U: -0.126994, V: -0.587267, angle: 0.348214, I_q: -0.663006, I_d: -0.375778
